/*
 *  definitions.c
 *  roversimulator
 *
 *  Created by Matt Roman on 8/28/08.
 *  Copyright 2008 University of Oklahoma. All rights reserved.
 *
 */

#include <math.h>
#include "definitions.h"

Point3D MakePoint(double a,double b,double c)
{
	Point3D pt;
	pt.x = a;
	pt.y = b;
	pt.z = c;
	return pt;
}
Vector3D MakeVector(double a,double b,double c)
{
	Vector3D vt;
	vt.x = a;
	vt.y = b;
	vt.z = c;
	return vt;
}

double magnitude3d(Point3D p,Point3D q)
{
	return sqrt((p.x - q.x)*(p.x - q.x) + (p.y - q.y)*(p.y - q.y) + (p.z - q.z)*(p.z - q.z));
}
double dot(Vector3D V1,Vector3D V2)
{
	double dot;
	dot = (V1.x * V2.x) + (V1.y * V2.y) + (V1.z * V2.z);
	return dot;
}
double dotVP(Vector3D V,Point3D P)
{
	double dot;
	dot = (V.x * P.x) + (V.y * P.y) + (V.z * P.z);
	return dot;
}
void generateRotationFrame(frameVectors *FV)
{
	Vector3D tempX,tempY,tempZ;
	
	tempX.x = FV->X.x;
	tempX.y = FV->Y.x;
	tempX.z = FV->Z.x;
	tempY.x = FV->X.y;
	tempY.y = FV->Y.y;
	tempY.z = FV->Z.y;
	tempZ.x = FV->X.z;
	tempZ.y = FV->Y.z;
	tempZ.z = FV->Z.z;
	
	FV->X = tempX;
	FV->Y = tempY;
	FV->Z = tempZ;
}
float cross(Vector3D V1,Vector3D V2, Vector3D*N)
{
	N->x = V1.y*V2.z - V1.z*V2.y;
	N->y = V1.z*V2.x - V1.x*V2.z;
	N->z = V1.x*V2.y - V1.y*V2.x;
	return N->z;
}
// calculates the normalized cross product of v1 to v2 and places it in v3
float NormalizeCross(Vector3D *v1, Vector3D *v2, Vector3D *v3)
{
	float r;
	// a x b = ((a2b3 - a3b2),(a3b1 - a1b3),(a1b2 - a2b1))
	
	v3->x = v1->y*v2->z - v1->z*v2->y;
	v3->y = v1->z*v2->x - v1->x*v2->z;
	v3->z = v1->x*v2->y - v1->y*v2->x;
	
	r = sqrt((v3->x*v3->x) + (v3->y*v3->y) + (v3->z*v3->z));
	
	v3->x /= (float)r;
	v3->y /= (float)r;
	v3->z /= (float)r;
	
	return v3->z;
}
void pointsToVector(Point3D a,Point3D b,Vector3D*V)
{
	V->x = b.x - a.x;
	V->y = b.y - a.y;
	V->z = b.z - a.z;
}
void arraysToVector(float *a,float *b,Vector3D*V)
{
	V->x = *b - *a;
	V->y = *(b+1) - *(a+1);
	V->z = *(b+2) - *(a+2);
}

void defaultKonstants(void)
{
	rk.SPEEDLIMIT=10000;
	rk.ROVERCRUISESPEED=rk.SPEEDLIMIT;
	rk.POINTTURNSPEED=rk.SPEEDLIMIT/2;
	rk.POINTTURNANGLE=120.0;
	//turnmult is subtracted from the slow wheel. at pointturnangle it is a point turn, but moves straighter and straighter
	rk.TURNMULT=(2*rk.POINTTURNSPEED)/rk.POINTTURNANGLE;
	rk.DECLINATION=12.45;
	rk.WHEELDIAM=0.2;
	rk.LTICSPERREV=4200;
	rk.RTICSPERREV=4200;
	rk.ROVERWIDTH=0.5;
	rk.ROVERWIDTHINT=50;
	rk.TELEPORTDISTANCE=1.0;
	rk.CLOSEENOUGH=1.0;
	rk.NOPROGRESSTIME=600;
	rk.STALLPERCENTAGE=0.25;
	rk.TURNDIFF=700;
	rk.PTSTALLTIME=3000;
	rk.NEEDTURNCORRECTION=8.0;
	rk.TURNACCURACYLIMIT=2.0;
	rk.TURNINCREMENT=0.05;
	rk.JOYINCREMENT=50;
	rk.BODYDIST=1500;
	rk.PROFILEHEIGHT=0.46;
	rk.PANELHEIGHT=0.47;
	rk.PROFILEOBSTACLEMAX=0.15;
	rk.PROFILEOBSTACLEMIN=-0.15;
	rk.PROFILESEG=12;
	rk.PANELSEGMENTS=16;
	rk.PROFILEINCR=0.09817477;
	rk.BODYINCR=0.13499;
	rk.PANELINCR=0.13499;
	rk.MAXPITCH=15.0;
	rk.MAXROLL=20.0;
	rk.BODYTOOCLOSE=0.5;
	rk.FOROFFSETWHEELS=0.35;
	rk.FOROFFSETBODYLASER=0.265;
	rk.FOROFFSETPROFILE=0.415;
	rk.FOROFFSETPANELLASER=0.5;
	rk.FORPANELGROUNDINTERSECT=rk.FOROFFSETPANELLASER+rk.PANELHEIGHT;
	rk.MAXDISTMAP=30;
	rk.NAPTIME=10;
	rk.PATHEFFICIENCY=1.5;
	rk.TURNFACTOR=0.75;
	rk.GOPASTDIST=1.5;
	rk.PANELOBSTACLES=1;
	rk.PROFILEOBSTACLES=1;
	rk.BODYOBSTACLES=1;
	rk.SLOPEOBSTACLES=1;
	rk.PITCHDOWNIGNOREDISTOBSTACLES=-2;
}